#include <dalelib/alife/robotjoint.h>
#include <dalelib/alife/robot.h>

namespace dalelib
{

RobotJoint::RobotJoint() { }

RobotJoint::~RobotJoint() { dJointDestroy(_id); }
/*
void RobotJoint::setLimits(const float _LoStop, const float _HiStop)
{
	dJointSetHingeParam(_id, dParamLoStop, _LoStop);
	dJointSetHingeParam(_id, dParamHiStop, _HiStop);
}

void RobotJoint::setVelocity(const float _Velocity)
{
	dJointSetHingeParam(_id, dParamFMax, _Velocity*_strength);
	dJointSetHingeParam(_id, dParamVel, _Velocity);
}
*/
//**************************************************
RobotBallJoint::RobotBallJoint(Robot* _Parent, const dBodyID _B0, const dBodyID _B1, const v3 _Anchor)
{
	_type = BALLJOINT;
	_parent = _Parent;
	_body[0] = _B0;
	_body[1] = _B1;
	_id = dJointCreateBall(_parent->getWorld(), _parent->getJointGroup());
	dJointAttach(_id, _B0, _B1);
	dJointSetBallAnchor(_id, _Anchor[0], _Anchor[1], _Anchor[2]);
}

void RobotBallJoint::update() { }

void RobotBallJoint::draw() { }

//**************************************************
RobotHingeJoint::RobotHingeJoint(Robot* _Parent, const dBodyID _B0, const dBodyID _B1, const v3 _Anchor, const v3 _Axis, const float _Strength)
{
	_type = HINGEJOINT;
	_parent = _Parent;
	_id = dJointCreateHinge(_parent->getWorld(), _parent->getJointGroup());
	_body[0] = _B0;
	_body[1] = _B1;
	_strength = _Strength;
	dJointAttach(_id, _B0, _B1);
	dJointSetHingeAnchor(_id, _Anchor[0], _Anchor[1], _Anchor[2]);
	dJointSetHingeAxis(_id, _Axis[0], _Axis[1], _Axis[2]);
//	dJointSetHingeParam(_id, dParamLoStop, -PI/2);
//	dJointSetFeedback(_id,&_feedback);
}

void RobotHingeJoint::setAngle(const float _Angle, const float _Force)
{
	float angle = float(dJointGetHingeAngle(_id));
	dJointSetHingeParam(_id, dParamFMax, _Force*_strength);
	dJointSetHingeParam(_id, dParamVel, (_Angle-angle)*_strength);
}

void RobotHingeJoint::update()
{
	dJointGetFeedback(_id);
}

void RobotHingeJoint::draw()
{
	v3 anchor = getAnchor();
	v3 axis = getAxis();

//	glVertex3fv((anchor-axis).ptr());
	glVertex3fv((float*)(anchor).ptr());
	glVertex3fv((float*)(anchor+axis).ptr());
}

//**************************************************
RobotSliderJoint::RobotSliderJoint(Robot* _Parent, const dBodyID _B0, const dBodyID _B1, const v3 _Anchor, const v3 _Axis, const float _Strength)
{
	_type = SLIDERJOINT;
}

void RobotSliderJoint::update() { }

void RobotSliderJoint::draw() { }

//**************************************************
RobotUniversalJoint::RobotUniversalJoint(Robot* _Parent, const dBodyID _B0, const dBodyID _B1, const v3 _Anchor, const v3 _Axis, const float _Strength)
{
	_type = UNIVERSALJOINT;
}

void RobotUniversalJoint::update() { }

void RobotUniversalJoint::draw() { }


} // namespace dalelib
